/*
 * @Date: 2020-11-01 18:52:50
 * @LastEditors: Tonser
 * @LastEditTime: 2020-11-01 19:31:21
 * @FilePath: /swarm_ws/src/px4_swarm_control/src/SwarmStates.h
 * @Description: 集群状态管理
 */

#ifndef SRC_SWARMSTATES_H
#define SRC_SWARMSTATES_H

#include <vector>
#include <queue>
#include "px4_swarm_control/incoming_broadcast_data.h"
#include <px4_swarm_control/remote_uwb_info.h>


class SwarmStates {
public:
    struct UWBSwarmState{
        int id; // 当前节点id
        int systemTime; // 当前状态对应的时间
        std::vector<int> swarmIds; // 当前集已有的节点数
        std::vector<uint8_t> swarmActive; // 当前节点活跃情况
        std::vector<float> nodesDistanceUWB; // 当前测得的距离
        std::vector<int> recvDistanceTime; // 通过UWB生成的当前集群的的各个节点之间的距离
        int remoteNodeNum; // 远程节点的数目 ，可以用做简单的校验
    };
    void updateStateFromUWB(const px4_swarm_control::remote_uwb_info &info_msg);
    UWBSwarmState getState();
    std::queue<UWBSwarmState> stateQuene;
    UWBSwarmState currentState;
private:
    UWBSwarmState lastState;
};

void SwarmStates::updateStateFromUWB(const px4_swarm_control::remote_uwb_info &info_msg){
    currentState.swarmIds = info_msg.node_ids;
    currentState.swarmActive = info_msg.active;
    currentState.nodesDistanceUWB = info_msg.node_dis;
    currentState.systemTime = info_msg.sys_time;
    currentState.recvDistanceTime = info_msg.recv_distance_time;
    currentState.id = info_msg.self_id;
    currentState.remoteNodeNum = info_msg.active.size();

    // active 取上次的与本次信息的 或值，因为偶尔回出现没有检测到的情况
    if(lastState.swarmActive.size()==currentState.swarmActive.size()){
        for (int i = 0; i < currentState.swarmActive.size(); i++)
        {
            currentState.swarmActive[i] = currentState.swarmActive[i] || lastState.swarmActive[i];
        }
    }
    lastState.swarmIds = info_msg.node_ids;
    lastState.swarmActive = info_msg.active;
    lastState.nodesDistanceUWB = info_msg.node_dis;
    lastState.systemTime = info_msg.sys_time;
    lastState.recvDistanceTime = info_msg.recv_distance_time;
    lastState.id = info_msg.self_id;
    lastState.remoteNodeNum = info_msg.active.size();
}


#endif //SRC_SWARMSTATES_H
